#include "realsense.h"

using namespace std;
#define NO_ERROR 0
#define PARM_INVAILD EINVAL

/**
 * @brief RealsenseDetector:构造函数
 */
RealsenseDetector::RealsenseDetector(int movementFd_in)
{
    dev_status = 0;
    m_movementFd = movementFd_in;
}

/**
 * @brief ~RealsenseDetector:析构函数
 */
RealsenseDetector::~RealsenseDetector()
{
    close_device();
}

/**
 * @brief open_device: 打开设备
 * @param pipeline_mode:选择打开设备的管道方式 取值:0(默认)，1(OpenGL),2(OpenCL)
 * @return 成功: NO_ERROR, 失败: 非0错误码
 */
int RealsenseDetector::open_device()
{
    if (dev_status == 1) {
        printf("RealsenseDetector: open_device(): dev_status == 1, return immediately!");
        return NO_ERROR;
    }
    
    //cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 15);//
    cfg.enable_stream(RS2_STREAM_DEPTH, FRAME_WIDTH, FRAME_HEIGHT, RS2_FORMAT_Z16, FRAME_FPS);

    try {
         pipe.start(cfg);
         printf("RealSense pipe.start ok\n");
    } catch(const rs2::error &e){
	std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    	return EXIT_FAILURE;
    }
    catch (const std::exception & e) {
        std::cerr << e.what() << std::endl;
        return EXIT_FAILURE;
    }	

    dev_status = 1;
    return NO_ERROR;
}

/**
 * @brief close_device: 关闭设备函数
 * @return 成功: NO_ERROR, 失败: 非0错误码
 */
int RealsenseDetector::close_device()
{
    if (dev_status > 0) {
        dev_status = 0;
#if Display_Mode
        //destroyAllWindows();
#endif
    }
    pipe.stop();
    cfg.disable_stream(RS2_STREAM_DEPTH);
    cfg.disable_stream(RS2_STREAM_COLOR);
    return NO_ERROR;
}

/**
 * @brief stop_device: 暂停设备
 * @return 成功: NO_ERROR, 失败: 非0错误码
 */
int RealsenseDetector::stop_device()
{
    if (dev_status == 0 || dev_status == 2) {
        printf("RealsenseDetector: stop_device(): dev_status == 0 || dev_status == 2, return immediately!");
        return NO_ERROR;
    }
    cfg.disable_stream(RS2_STREAM_DEPTH);
    dev_status = 2;
    return NO_ERROR;
}

/**
 * @brief continue_device: 快速重启设备
 * @return 成功: NO_ERROR, 失败: 非0错误码
 * @note 只是快速重启，没有重新设定参数的功能
 *        要重新设定参数请关闭设备再打开
 */
int RealsenseDetector::start_device()
{
    if (dev_status == 3) {
        printf("start_device(): start_device(): dev_status == 3, return immediately!");
        return NO_ERROR;
    }

#if Video_Output
//    myVideo.run_my_thread(8, thread_no_reg_copy, this, sizeof(this));
#endif
    dev_status = 3;
    return NO_ERROR;
}

int RealsenseDetector::get_status()
{
    return dev_status;
}

int RealsenseDetector::get_range(int index, int *x1, int *x2, int *y1, int *y2)
{
    int zone_width = FRAME_WIDTH / X_ZONE_NUM;
    *x1 = index * zone_width;
    *x2 = (index+1) * zone_width;
    *y1 = FRAME_HEIGHT * Y_TOP_RATE;
    *y2 = FRAME_HEIGHT * (1 - Y_BOT_RATE);
    return 0;
}

void RealsenseDetector::get_xrange(int xindex, int *x1, int *x2)
{
    int zone_width = FRAME_WIDTH / X_ZONE_NUM;
    *x1 = xindex * zone_width;
    *x2 = (xindex+1) * zone_width;
}

void RealsenseDetector::get_yrange(int yindex, int *y1, int *y2)
{
    int zone_height = FRAME_HEIGHT / Y_ZONE_NUM;
    *y1 = yindex * zone_height;
    *y2 = (yindex+1) * zone_height;
}

int RealsenseDetector::roi_cloud(cv::Mat& src, cv::Mat& src_roi, int cam_heigh, int cam_side)
{
	int roi_num1 = 0, roi_num2 = 0, roi_num3 = 0, roi_num4 = 0, roi_num5 = 0;
	int roi_down = 0, roi_down1 = 0, roi_up = 0;
	double roi_porp = 0;

	int roi_rows1 = src.rows/2, roi_rows2 = src.rows;
	int roi_cols1 = src.cols/8, roi_cols2 = 7*src.cols/8;
    for (int i = roi_rows1; i<roi_rows2; i++) {
		ushort* val_dep_ptr = src.ptr<ushort>(i);
        for (int j = roi_cols1; j<roi_cols2; j++) {
			ushort val_dep = val_dep_ptr[j];

      	    if (val_dep > 5000 || val_dep < 800)
        	    continue;
				
			double x_val = (j - cam_para.cx) * val_dep / cam_para.fx;
			double y_val = (i - cam_para.cy) * val_dep / cam_para.fy;
			if (y_val > cam_heigh - 70)
				continue;
			
			if (x_val < -cam_side || x_val > cam_side) //0.7m
				continue;

			if (y_val < cam_heigh - 70 && y_val > cam_heigh - 140) {
				src_roi.at<ushort>(i, j) = val_dep;
				roi_num1++;
			}

			if (y_val < cam_heigh - 110 && y_val > cam_heigh - 180) {
				src_roi.at<ushort>(i, j) = val_dep;
				roi_num2++;
			}

			if (y_val < cam_heigh - 150 && y_val > cam_heigh - 220) {
				src_roi.at<ushort>(i, j) = val_dep;
				roi_num3++;
			}

			if (y_val < cam_heigh - 230 && y_val > cam_heigh - 300) {
				src_roi.at<ushort>(i, j) = val_dep;
				roi_num4++;
			}

			if (y_val < cam_heigh - 270 && y_val > cam_heigh - 340) {
				src_roi.at<ushort>(i, j) = val_dep;
				roi_num5++;
			}
		}
	}

	roi_down1 = max(roi_num1, roi_num2);
    roi_down = max(roi_down1, roi_num3);
	roi_up = max(roi_num4, roi_num5);
	roi_porp = roi_down * 1.0 / roi_up;
	std::cout<<"roi_num1: "<<roi_num1<<", roi_num2: "<<roi_num2<<", roi_num3: "<<roi_num3<<", roi_num4: "<<roi_num4<<"，roi_num5: "<<roi_num5<<std::endl;
	std::cout<<", roi_down: "<<roi_down<<", roi_up: "<<roi_up<<", roi_porp: "<<roi_porp<<std::endl;
}

bool RealsenseDetector::point_angle_trans(cv::Mat& src, int angle, int height, int height_max)
{
    cv::Mat src_angle(src.rows, src.cols, src.type(), Scalar::all(0));
    cv::Mat src_fall(src.rows, src.cols, src.type(), Scalar::all(0));
	double angle_h = angle * 3.1415926 / 180;
	int height_m = height_max - height;
	double cos_angle = cos(angle_h);
	double sin_angle = sin(angle_h);
	double cam_para_yb = 1.0 / cam_para.fy;

    for (int i = 0; i < src.rows; i++) {
		ushort* val_dep_ptr = src.ptr<ushort>(i);
		ushort* val_angle_ptr = src_angle.ptr<ushort>(i);
		ushort* val_fall_ptr = src_fall.ptr<ushort>(i);
        for (int j = 0; j < src.cols; j++) {
			ushort val_dep = val_dep_ptr[j];

            if (val_dep > 4000 || val_dep < 200) {
				//src.at<ushort>(i, j) = 0;
        	    continue;
		    }

			double y_val = (i - cam_para.cy) * val_dep * cam_para_yb;
			//double y_val_trans = y_val * cos(angle_h) + val_dep * sin(angle_h);
			double y_val_trans = y_val * cos_angle + val_dep * sin_angle;

			if(y_val_trans > height + 360) {  // test
				val_fall_ptr[j] = val_dep;
				//src_fall.at<ushort>(i, j) = val_dep;
				//cout<<"y_val_trans: "<<y_val_trans<<endl;
				continue;
			}

			if(y_val_trans > height - 80 || y_val_trans < -height_m) { //|| y_val_trans < -1000) 120
				//src.at<ushort>(i, j) = 0;
				continue;
			}

			//double val_trans = val_dep * cos(angle_h) - y_val * sin(angle_h);
			double val_trans = val_dep * cos_angle - y_val * sin_angle;

			//src_angle.at<ushort>(i, j) = (ushort)val_trans;
			val_angle_ptr[j] = (ushort)val_trans;
        }
    }
    int count = filer_noise_test(src_fall, 20, 0.7);//(20 or 30)
    src_angle.copyTo(src);

    if(count > 40) {
        printf("filer_noise_test: %d\n", count);
        return true;
    }
    return false;
}

void RealsenseDetector::get_threshold_mask(Mat &src, Mat& thr, int thr_min, int thr_max)
{
	src.copyTo(thr);
    for (int i = 0; i < src.rows; i++) {
		ushort* val_dep_ptr = thr.ptr<ushort>(i);
        for (int j = 0; j < src.cols; j++) {
			ushort val_dep = val_dep_ptr[j];
            if (val_dep > thr_max || val_dep <= thr_min){
            	//thr.at<ushort>(i, j) = 0;
				val_dep_ptr[j] = 0;
			}
        }
    }
}

Mat gray_to_color(Mat &gray)
{
    Mat dispRgb(gray.rows, gray.cols, CV_8UC4);
    Vec4b red(0, 0, 255), green(0, 255, 0), blue(255, 0, 0), purple(255, 0, 255),black(0, 0, 0), yellow(0, 255, 255), white(255, 255, 255), cyan(255, 255, 0), cont(125, 0, 125);
    for(int i = 0; i < gray.rows; i++){
        for(int j = 0; j < gray.cols; j++){
            uchar grayVal = gray.at<uchar>(i,j);
            //printf("gray %d ",grayVal);
	        if(grayVal==0 || grayVal > 400){
 		        dispRgb.at<Vec4b>(i,j) =black;
	        } else if(grayVal< WARN_LIMIT*0.5 * 0.1){
 		        dispRgb.at<Vec4b>(i,j) =red;
	        }else if(grayVal< WARN_LIMIT*0.8* 0.1){
                dispRgb.at<Vec4b>(i,j) =yellow;
            }else if(grayVal< WARN_LIMIT*1.1 * 0.1){
                dispRgb.at<Vec4b>(i,j) =green;
            }else if(grayVal< WARN_LIMIT*1.4 * 0.1){
                dispRgb.at<Vec4b>(i,j) =blue;
            }else if(grayVal< WARN_LIMIT*1.8 * 0.1){
                dispRgb.at<Vec4b>(i,j) = purple;
            }else if(grayVal < WARN_LIMIT*2.2 * 0.1){
                dispRgb.at<Vec4b>(i,j) = cyan;
            }else if(grayVal < WARN_LIMIT*2.5 * 0.1){
                dispRgb.at<Vec4b>(i,j) = cont;
            }else if(grayVal > WARN_LIMIT*2.5 * 0.1){
                dispRgb.at<Vec4b>(i,j) = white;
            }
	    }
    }
    //printf("\n");
    // imshow("depth RGB", dispRgb);
    return dispRgb;
}

bool RealsenseDetector::ward_cam_obj(Mat &dep_src, double inval_pre)
{
    static int start_num = 0;
    if(start_num < 20) {
        start_num++;
        return false;
    }

    int count1 = 0, count2 = 0;
    for(int i = 0; i < dep_src.rows; i += 6) {
		ushort* val_dep_ptr = dep_src.ptr<ushort>(i);
        for(int j = 0; j < dep_src.cols; j += 6) {
            ushort val_dep = val_dep_ptr[j];
            count1++;
            if(val_dep == 0) {
                count2++;
            }
        }
    }

    double val_pre = count2 * 1.0 / count1;
    if(val_pre > inval_pre) {
        printf("val_pre:%f\n", val_pre);
        return true;
    }
    return false;
}

int RealsenseDetector::filer_noise_test(Mat &src, int element_size, double val_prop)
{
    int element_half = element_size / 2;
    int step_num = element_size;
    int count = 0;

    for(int i = element_half; i <= src.rows - element_half; i = i + step_num){
        for(int j = element_half; j <= src.cols - element_half; j = j + step_num){
            if(j == 1245 && element_size == 30) {
                j = j - 10;
            }

			cv::Rect rect;
			rect.x = j - element_half;
			rect.y = i - element_half;
			rect.width = 2 * element_half;
			rect.height = 2 * element_half;

			int val_num_eff = cv::countNonZero(src(rect));
			int val_num_all = rect.width * rect.height;
			double val_prop_now = val_num_eff * 1.0 / val_num_all;

			cv::Mat all_zero(rect.height, rect.width, src.type(), cv::Scalar::all(0));
			cv::Mat tmp_mat = src(rect);
            if(val_prop_now < val_prop){
				all_zero.copyTo(tmp_mat);
            }
            else {
                count++;
            }
        }
    }

    return count;
}

bool RealsenseDetector::fall_down_obj(Mat &src, int element_size, double val_prop)
{
    cv::Mat dep_fall(src.rows, src.cols, src.type(), Scalar::all(0));
    int element_half = element_size / 2;
    int step_num = element_size;
	int count1 = 0, count2 = 0;
    int row_num = 4;

    for(int i = src.rows-1; i >= src.rows - row_num*element_size; i--) {
        ushort* val_dep_ptr = src.ptr<ushort>(i);
        ushort* val_fall_ptr = dep_fall.ptr<ushort>(i);
        for(int j = 0; j < src.cols; j++) {
            ushort val_dep = val_dep_ptr[j];
            if (val_dep < 3500 && val_dep > 200) {
        	    //dep_fall.at<ushort>(i, j) = val_dep;
				val_fall_ptr[j] = val_dep;
		    }
        }
    }

    for(int i = dep_fall.rows - element_half; i >= dep_fall.rows - row_num*element_half; i -= step_num) {
        for(int j = element_half + 2*step_num; j <= dep_fall.cols - element_half; j += step_num) {
            if(j == 1245 && element_size == 30) {
                j = j - 10;
            }
            
			cv::Rect rect;
			rect.x = j - element_half;
			rect.y = i - element_half;
			rect.width = 2 * element_half;
			rect.height = 2 * element_half;

			int val_num_eff = cv::countNonZero(src(rect));
			int val_num_all = rect.width * rect.height;

			count1++;
            double val_prop_now = val_num_eff * 1.0 / val_num_all;
            if(val_prop_now > val_prop) {
				ushort val_fall = src.at<ushort>(i, j);
				int n = 2;
				for(int k = 1; k < n; k++) {
					val_fall = val_fall + src.at<ushort>(i+k, j) + src.at<ushort>(i, j+k);
					val_fall = val_fall + src.at<ushort>(i-k, j) + src.at<ushort>(i, j-k);
				}
				int val_fall_m = val_fall / (4*(n-1)+1);
				if(val_fall_m > 3200 || val_fall_m < -3200) {
					count2++;
					//printf("i:%d, j:%d, val_prop:%f, val_fall_m:%d\n", i, j, val_prop_now, val_fall_m);
				}
            }
        }
    }

    //dep_fall.copyTo(src);
	double val_pre = count2 * 1.0 / count1;
	if (val_pre > 0.12) {
		printf("count2: %d, pre: %f\n", count2, val_pre);
        return true;
	}
	return false;
}


int RealsenseDetector::process_depth_data(Mat &depth)
{
    Mat dep, dep_mask, dep_src, dep_mat;
    cv::Mat dep_angle(depth.rows, depth.cols, depth.type(), Scalar::all(0));

	double time_start = static_cast<double>(getTickCount());
	depth.copyTo(dep_src);
	depth.copyTo(dep_mat);

    bool fall_down = false, ward_cam = false;
    fall_down = point_angle_trans(depth, 0, 750, 2000);
	//double time = (static_cast<double>(getTickCount()) - time_start) / getTickFrequency();
	//std::cout<<"time1: "<<time<<std::endl;

    if(!fall_down) {
	    fall_down = fall_down_obj(dep_src, 20, 0.98);
    }
    if(fall_down) {
        printf("front has fall down angle\n");
    }

    ward_cam = ward_cam_obj(dep_src, 0.7);
    if(ward_cam) {
        printf("cam has ward\n");
    }

    get_threshold_mask(depth, dep, 200, 1000);//0.2m~1.0m
    get_threshold_mask(depth, dep_mask, 1000, 1800);//1.0m~1.8m

    int scale = 10;
    filer_noise_test(dep, 20, 0.7);//(20 or 30)
    filer_noise_test(dep_mask, 30, 0.8);
	cv::add(dep, dep_mask, dep);
    dep.convertTo(dep, CV_8UC1, 1.0 / scale);
    Mat out = dep;
/*
    Mat resize = dep;
    Mat element = getStructuringElement(MORPH_RECT, Size(15, 15));//核的大小可适当调整 15 15
    //进行开操作
    morphologyEx(dep, out, MORPH_OPEN, element);
    //dilate(out, out, element);
*/
#if Display_Mode
    //显示效果图
//    imshow("opencv-out", out);
	//imwrite("./depth_open.jpg", dep);
    //imshow("opencv", dep);
#endif

    Mat color = gray_to_color(out);
    const uchar max_dist_const = 250;//2.5m
    int xindex, yindex, i, j, x1, x2, y1, y2;
	int detect_num = 0;
	double x_distance = 0;

    for (xindex=0; xindex<X_ZONE_NUM; xindex++) {
        //get_range(xindex, &x1, &x2, &y1, &y2);
		get_xrange(xindex, &x1, &x2);
        uchar max_dist = 0, min_dist = max_dist_const; //2.5m

		for(yindex=0; yindex<Y_ZONE_NUM; yindex++) {
			get_yrange(yindex, &y1, &y2);

	    	for( i = y1; i< y2; i+=Y_DIVIDE) {
            	const uchar *cur = out.ptr<uchar>(i);
				ushort* src_value_ptr = dep_mat.ptr<ushort>(i);
            	for (j=x1; j<x2; j+=X_DIVIDE) {
		        	uchar val = cur[j];
                	//ushort src_value = dep_mat.at<ushort>(i, j) / scale;

		        	if(val > 20 && val < max_dist_const){ // >20cm is the valid data
		            	if(val> max_dist) {
			            	max_dist = val;
		            	} else if(val < min_dist) {
			            	min_dist = val;
            				x_distance = (j - cam_para.cx) * (src_value_ptr[j]/scale) / cam_para.fx;
		            	}
		        	}
	        	}
	   		}
        	if (min_dist > max_dist_const) {
            	mix_pack.info_pack[detect_num].min_depth = 2500; //比最大值大的范围
				
        	} else {
            	mix_pack.info_pack[detect_num].min_depth = min_dist*scale;
            	mix_pack.info_pack[detect_num].x_distance = x_distance*scale;
	    	}
			cout<<"detect_num: "<<detect_num<<", min_dist: "<<(int)min_dist<<", x_distance: "<<(int)x_distance<<endl;
			detect_num++;

#if Display_Mode
    		cv::line(color, Point(x1, y1), Point(x1, y2), (255, 0, 255));
    		cv::line(color, Point(0, y1), Point(FRAME_WIDTH, y1), (255, 0, 255));
        	float minDist = min_dist*0.01;
        	char strDist[128]={0};
 	    	sprintf(strDist,"%.2fm",minDist);
	    	std::string text = strDist;
        	int font_face = cv::FONT_HERSHEY_COMPLEX; 
        	double font_scale = 0.7;
        	int thickness = 1;
        	cv::Point origin; 
        	origin.x = x1;
	    	origin.y = y1;
	    	//cv::putText(color, text, origin, font_face, font_scale, cv::Scalar(255, 255, 255), thickness, 1, 0);
#endif
		}
    }

#if Display_Mode
    imshow("depth RGB", color);
    waitKey(1);
#endif

    return 0;
}


int RealsenseDetector::process_one_frame()
{
    // Using the align object, we block the application until a frameset is available
    rs2::frameset frameset = pipe.wait_for_frames();
    auto depth = frameset.get_depth_frame();
    const int w = depth.as<rs2::video_frame>().get_width();
    const int h = depth.as<rs2::video_frame>().get_height();
    cv::Mat image(Size(w, h), CV_16UC1 , (void*)depth.get_data(), cv::Mat::AUTO_STEP);
    process_depth_data(image);
}

int RealsenseDetector::Robot_Send_Kinect_Info(depth_info_mix &mix)
{
    int ret = -1;
#ifndef PC_Mode
    uchar package[4+X_ZONE_NUM*2];
    package[0] = 0xaa;
    package[1] = 0x53;
    int i;
    for (i=0; i<X_ZONE_NUM; i++) {
        package[2+i] = mix.info_pack[i].min_depth/20;
        //package[2+2*i+1] = mix.info_pack[i].avr/20;
        //package[2+2*i+1] = mix.info_pack[i].z_xhypo_angle/2;
    }
    int len_in = sizeof(package) - 2;
    uchar send_sum = cal_sum(package, len_in);
    package[len_in] = send_sum;
    package[len_in+1] = '*';

    Movement::write_lock.lock();
    ret = serial_write(m_movementFd, len_in+2, package);
    if (ret != NO_ERROR) {
        Movement::write_lock.unlock();
        return ret;
    }
    Movement::write_lock.unlock();
#else
    //只为了屏蔽编译警告
    if (mix.info_pack[0].min_depth == 0)
        ret = NO_ERROR;
#endif
    return ret;
}

/**
 * @brief 计算校验和
 * @param data: 要计算校验和的数据
 * @param n: 要计算校验和的数据个数
 * @return 校验和
 */
uchar RealsenseDetector::cal_sum(uchar *data, int n)
{
    int i;
    uchar sum = 0;
    for (i=0; i<n; i++)
        sum += data[i];
    return sum;
}

void RealsenseDetector::get_kinect_info_pack(depth_info_mix &info_pack)
{
    //lock.lock();
    info_pack = mix_pack;
    //lock.unlock();
}

int RealsenseDetector::depth_to_gray(Mat &src, Mat &dest)
{
    if ((src.size == 0) || (dest.size == 0)) {
        printf("RealsenseDetector: PARM_INVAILD: (src.size == 0) || (dest.size == 0)!");
        return PARM_INVAILD;
    }

    int nr = src.rows, nc = src.cols;
    dest = Scalar_<int>(255);
    int i, j;
    const float *src_ptr;
    unsigned char *dest_ptr;
    float val = 0;
    double alphas = 255/4500;
    for (i=0; i<nr; i++) {
        src_ptr = (const float*)(src.data + i * src.step);
        dest_ptr = dest.data + i * dest.step;
        for (j=0; j<nc; j++) {
            val = *(src_ptr + j);
            if (val != 0) {
                *(dest_ptr + j) = val*alphas;
            }
        }
    }

    return NO_ERROR;
}

